using BepuUtilities;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 定义运动约束中的某些共享行为。
    /// </summary>
    public struct MotorSettings
    {
        /// <summary>
        /// 马达在一个单位时间内可以施加的最大力。
        /// </summary>
        public float MaximumForce;
        /// <summary>
        /// 质量定标阻尼常数。如果要使用质量为M的对象模拟D的粘性阻尼系数,请将该阻尼值设置为D/M。
        /// </summary>
        public float Damping;

        /// <summary>
        /// 获取或设置约束的软程度。值的范围从0到无穷大。柔和度是反向阻尼;0是完全刚性的,1是非常软的,浮动。MaxValue实际上是不存在的。
        /// </summary>
        public float Softness { get { return 1f / Damping; } set { Damping = value <= 0 ? float.MaxValue : 1f / value; } }

        /// <summary>
        /// 检查设置实例是否具有有效的非负值。
        /// </summary>
        /// <param name="settings">要检查的实例。</param>
        /// <return>如果设置有效,则为True,否则为False。</return>
        public static bool Validate(in MotorSettings settings)
        {
            return ConstraintChecker.IsNonnegativeNumber(settings.MaximumForce) && ConstraintChecker.IsNonnegativeNumber(settings.Damping);
        }

        /// <summary>
        /// 定义运动约束的设置。
        /// </summary>
        /// <param name="maxumForce">马达在一个单位时间内可以施加的最大力。</param>
        /// <param name="softness">获取或设置约束的软程度。值的范围从0到无穷大。柔和度是反向阻尼;0表示完全刚性,1表示非常软,浮动。MaxValue实际上不存在。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MotorSettings(float maximumForce, float softness) : this()
        {
            MaximumForce = maximumForce;
            Softness = softness;
            Debug.Assert(Validate(this), "Motor settings must have nonnegative maximum force and nonnegative damping.");
        }
    }
    public struct MotorSettingsWide
    {
        public Vector<float> MaximumForce;
        public Vector<float> Damping;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WriteFirst(in MotorSettings source, ref MotorSettingsWide target)
        {
            GatherScatter.GetFirst(ref target.MaximumForce) = source.MaximumForce;
            GatherScatter.GetFirst(ref target.Damping) = source.Damping;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ReadFirst(in MotorSettingsWide source, out MotorSettings target)
        {
            target.MaximumForce = source.MaximumForce[0];
            target.Damping = source.Damping[0];
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeSoftness(in MotorSettingsWide settings, float dt, out Vector<float> effectiveMassCFMScale, out Vector<float> softnessImpulseScale, out Vector<float> maximumImpulse)
        {
            // 速度马达不能用阻尼比,没有位置目标,所以没有临界阻尼这回事。
            // 相反,我们从阻尼常数开始。
            // 然而,正如我们使用阻尼比和频率来避免处理质量调节一样,用户可以调节阻尼常数*除以质量*。
            // D=用户阻尼常数,表示预分频值
            // CFm=1/原始阻尼常数
            // CFM=(d*有效质量)^-1=有效质量^-1*d^-1
            // 软化有效质量=(有效质量^-1+CFM*dt^-1)^-1
            // 软化有效质量=(有效质量^-1+有效质量^-1*(d*dt)^-1)^-1
            // 软化有效质量=(有效质量^-1*(1+(d*dt)^-1))^-1
            // 软化有效质量=(1+(d*dt)^-1)^-1*有效质量
            // 软化有效质量=有效质量/(1+1/(d*dt))

            // 对于累积脉冲缩放组件：
            // 脉冲=偏置*软化有效质量-累积脉冲*cfm/dt*软化有效质量-wsv*jt*软化有效质量
            // 关注柔软度术语：
            // CFm/dt*softenedEffectiveMass=(有效质量^-1/(d*dt))*有效质量/(1+1/(d*dt))
            // CFm/dt*softenedEffectiveMass^-1*(有效质量/(1+1/(d*dt)/(d*dt)
            // CFm/dt*softenedEffectiveBulk=1/(1+1/(d*dt))/(d*dt)
            // CFm/dt*softenedEffectiveMass=1/(d*dt+1)
            // (有关详细信息,请参见Inequality1DOF示例约束。)

            var dtd = dt * settings.Damping;
            softnessImpulseScale = Vector<float>.One / (dtd + Vector<float>.One);
            effectiveMassCFMScale = dtd * softnessImpulseScale;
            maximumImpulse = settings.MaximumForce * dt;
        }
    }
}
